#include "motor_control.h"
#include "pc_interaction.h"
#include "can_cfg.h"
#include "semphr.h"
#include "remote_control_decode.h"

extern float Joint1_real_callback_ang;
extern float Joint2_real_callback_ang;
extern uint8_t ctr_cmd;
extern uint8_t ctr_ID;
extern uint8_t motor_type;
extern uint16_t ctr_abs_ang;
extern uint8_t Simulate_Human_Mode;
extern int16_t rev_pc_data_length;
uint8_t Fixed_trajectory = 0;
float test_err_num = 0;
extern osSemaphoreId_t joint_ang_semHandle;
extern RC_channelTypedef RC_channel_1;
extern RC_channelTypedef RC_channel_2;
extern uint8_t remote_ctr_mode;

//脉塔电机单字节控制 RMD_Motor_Normal_Ctr(ctr_ID, ctr_cmd);
//脉塔电机闭环绝对位置控制 RMD_Mul_abs_loop_angleControl(ctr_ID, RMD_Max_SPEED, ctr_abs_ang);
//关节电机单字节控制 Joint_CAN_Single_Command_Ctr(ctr_ID, ctr_cmd);
//关节电机位置控制 Joint_CAN_SendPosition(ctr_ID, ctr_abs_ang);

void Motor_10ms_control(void)
{
    if(motor_type == slave_rev_RMD_Motor)
    {
        if(rev_pc_data_length == 5)
        {
            //直接控制
            RMD_Motor_Normal_Ctr(ctr_ID, ctr_cmd);
        }
        else if(rev_pc_data_length > 5)
        {
            if(ctr_cmd == RMD_Pos_Closed_Loop_Control_Command_2)
            {
                RMD_Mul_abs_loop_angleControl(ctr_ID, RMD_Max_SPEED, ctr_abs_ang);
            }
        }
    }
    else if(motor_type == slave_rev_Joint_Motor)
    {
        if(rev_pc_data_length == 5)
        {
            //直接控制
            Joint_CAN_Single_Command_Ctr(ctr_ID, ctr_cmd);
        }
        else if(rev_pc_data_length > 5)
        {
            if(ctr_cmd == Joint_Set_Pos_Model_Pos_Angle_Command)
            {
                Joint_CAN_SendPosition(ctr_ID, ctr_abs_ang); 
            }
        }
    }
    else if(motor_type == slave_rev_Mix_Motor)
    {
        switch(Simulate_Human_Mode)
        {
            case(Reset_Motor):{
                Joint_CAN_SendPosition(slave_rev_Joint1_ID, Joint_1_Init_ang);
                Joint_CAN_SendPosition(slave_rev_Joint2_ID, Joint_2_Init_ang);
            };break;
            case(Forward_90_Command):{
                Joint_CAN_SendPosition(slave_rev_Joint1_ID, Joint_1_Forward_90_ang);
            };break;
            case(Turn_Right_90_Command):{
                Joint_CAN_SendPosition(slave_rev_Joint2_ID, Joint_2_Turn_Right_90_ang);
            };break;
            case(Inclined_Top_90_Command):{
                Joint_CAN_SendPosition(slave_rev_Joint1_ID, Joint_1_Forward_90_ang);
                Joint_CAN_SendPosition(slave_rev_Joint2_ID, Joint_2_Turn_Right_45_ang);
            };break;
            case(Draw_Triangular):{
                //
            };break;
            case(Draw_circle):{
                //
            };break;  
            case(Fixed_trajectory_cruise_Command):{
                // if(xSemaphoreTake(joint_ang_semHandle, portMAX_DELAY) == pdTRUE) //等同pdPASS
                // {
                    if(Fixed_trajectory == 0)
                    {
                        Joint_CAN_SendPosition(slave_rev_Joint1_ID, Joint_1_Forward_50_ang);
                        test_err_num = abs((int)(Joint1_real_callback_ang - 230.0));
                        if(test_err_num < 2)
                            Fixed_trajectory = 1;
                    }

                    if(Fixed_trajectory == 1 && abs((int)(Joint1_real_callback_ang - 230)) < 1) 
                    {
                        Joint_CAN_SendPosition(slave_rev_Joint2_ID, Joint_2_Turn_Right_60_ang);
                        Fixed_trajectory = 2;
                    }

                    if(Fixed_trajectory == 2 && abs((int)(Joint1_real_callback_ang - 230)) < 1 && abs((int)(Joint2_real_callback_ang - 120)) < 1)
                    {
                        Joint_CAN_SendPosition(slave_rev_Joint1_ID, Joint_1_Init_ang);
                        Joint_CAN_SendPosition(slave_rev_Joint2_ID, Joint_2_Init_ang);
                        Fixed_trajectory = 3;
                    }

                    if(abs((int)(Joint1_real_callback_ang - 180)) < 1 && abs((int)(Joint2_real_callback_ang - 180)) < 1 && Fixed_trajectory == 3)
                    {
                        Fixed_trajectory = 0;
                    }
                // }
                
            };break;
            default:break;
        }
    }

    else if(remote_ctr_mode == 1){
        // 遥控器解析数值小角度跳动 导致原点不能恒定
        // static uint8_t i = 0;
        // switch(i){
        //     case(0):Joint_CAN_SendPosition(slave_rev_Joint2_ID, RC_channel_1.real_Ctr_ang);break;
        //     case(1):Joint_CAN_SendPosition(slave_rev_Joint1_ID, RC_channel_2.real_Ctr_ang);break;
        // }
        // i ++;
        // if(i >= 2) i = 0;
        
        
    }
}


